#include "dji_linux_helpers.hpp"
#include "dji_linker.hpp"
#include "GSLAM/core/GSLAM.h"
#include "RTMapperNetInterface.h"

using namespace GSLAM;
using namespace sv;


class VehicleNode{
public:
    VehicleNode(Vehicle* vehicle, sv::Svar options)
        : vehicle(vehicle), options(options), workers(1){
        initialize();
    }

    ~VehicleNode(){
        shouldStop=true;
        while (workers.taskNumLeft()) {
            Rate::sleep(0.1);
        }
        LOG(INFO)<<"VehicleNode stoped.";
    }

    void initialize(){
        if(!vehicle) return;
	data=Svar::object();

	if(options.get("enable_rtmv",true))
          setupRTMV();;

	if(options.get("enable_photo",false))
          workers.Add([this](){ newPhotoNotify();});
    }

    void setupRTMV(){
        shouldStop=false;


        vehicle->broadcast->setBroadcastFreqDefaults(20);
        /*! init gimbal modules for gimbalManager */
        ErrorCode::ErrorCodeType ret;
        /*! main gimbal init */
        ret = vehicle->gimbalManager->initGimbalModule(DJI::OSDK::PAYLOAD_INDEX_0, "main_gimbal");

        if (ret != ErrorCode::SysCommonErr::Success)
        {
          std::cout << "Init Camera modules main_gimbal failed."<< std::endl;
          ErrorCode::printErrorCodeMsg(ret);
          return;
        }
        /*! vice gimbal init */
        ret = vehicle->gimbalManager->initGimbalModule(DJI::OSDK::PAYLOAD_INDEX_1, "vice_gimbal");
        if (ret != ErrorCode::SysCommonErr::Success)
        {
          std::cout << "Init Camera modules vice_gimbal failed." << std::endl;
          ErrorCode::printErrorCodeMsg(ret);
          return;
        }
        /*! top gimbal init */
        if (vehicle->isM300())
        {
          ret = vehicle->gimbalManager->initGimbalModule(DJI::OSDK::PAYLOAD_INDEX_2, "top_gimbal");
          if (ret != ErrorCode::SysCommonErr::Success) {
            std::cout << "Init Camera modules top_gimbal failed." << std::endl;
            ErrorCode::printErrorCodeMsg(ret);
            return;
          }
        }


        ret = vehicle->cameraManager->initCameraModule(DJI::OSDK::PAYLOAD_INDEX_0, "main_camera");
        if (ret != ErrorCode::SysCommonErr::Success) {
          //DERROR("Init Camera modules main_camera failed.");
          ErrorCode::printErrorCodeMsg(ret);
          return;
        }
        /*! vice camera init */
        ret = vehicle->cameraManager->initCameraModule(DJI::OSDK::PAYLOAD_INDEX_1, "vice_camera");
        if (ret != ErrorCode::SysCommonErr::Success) {
//          DERROR("Init Camera modules vice_camera failed.");
          ErrorCode::printErrorCodeMsg(ret);
          return;
        }
        /*! top camera init for M300 */
        if (vehicle->isM300()) {
          ret = vehicle->cameraManager->initCameraModule(DJI::OSDK::PAYLOAD_INDEX_2, "top_camera");
          if (ret != ErrorCode::SysCommonErr::Success)
          {
//            DERROR("Init Camera modules top_camera failed.");
            ErrorCode::printErrorCodeMsg(ret);
            return;
          }
        }

        LiveView::LiveViewCameraPosition pos   =options.get("camera_position",LiveView::OSDK_CAMERA_POSITION_NO_3);
        LiveView::LiveViewCameraSource   source=options.get("h264_source",LiveView::OSDK_CAMERA_SOURCE_H20T_WIDE);
        PayloadIndexType p_index= options.get("playload_index",PAYLOAD_INDEX_0);
        vehicle->cameraManager->getCameraModuleName(p_index,camera_name);

        if(vehicle->advancedSensing->changeH264Source(pos,source)!=0)
            LOG(WARNING) << "Failed to change h264 source.";

        vehicle->advancedSensing->startH264Stream(pos,[](uint8_t* buf, int bufLen, void* userData){
            VehicleNode* self=(VehicleNode*)userData;
            self->sendRTMV(buf,bufLen);
        },this);

        pub_rtmv=messenger.advertise<SvarBuffer>("rtmv");

//        if(vehicle == nullptr)
//        {
//          ROS_ERROR_STREAM("Vehicle modules is nullptr");
//          return true;
//        }




        std::thread th([this](){
            // wait accept thread
            Svar sockpp=GSLAM::Registry::load("svar_sockpp");

            if(!sockpp.isObject()){
                LOG(ERROR) <<"unable to load svar_sockpp.";
                return;
            }

            Svar init=sockpp["socket_initializer"]();

            Svar s = sockpp["tcp_acceptor"](options.get("port",1025));

            while(!shouldStop){
                tryAddClient(s);
            }
        });

        th.detach();
    }

    void tryAddClient(Svar s)
    {
        Svar dataCopy=data;

        //if(dataCopy.value().unique())
         //   return;


        LOG(INFO)<<"Ready to accept connections through"<<options.get("port",1025);
        Svar socket=s.call("accept");

        //if(dataCopy.value().unique())
         //   return;

        std::string client_addr=socket.call("peer_address").castAs<std::string>();

        auto sub= messenger.subscribe("rtmv",[socket,dataCopy,client_addr](Svar buf){
             int sent=socket.call("write",buf).as<int>();
             if(sent!=buf.as<SvarBuffer>().size()){
                 dataCopy["subs"].erase(client_addr);
		 LOG(INFO)<<"Client "<<client_addr<<" disconnected.";
             }
             else 
              DLOG(INFO)<<"sent "<<sent<<" to "<<client_addr;
        });

        dataCopy["subs"][client_addr] = sub;

        LOG(INFO)<<"added client:"<<client_addr;
    }

    void sendRTMV(uint8_t* buf, int bufLen){
        auto gimbal=vehicle->broadcast->getGimbal();
        auto gps   =vehicle->broadcast->getGlobalPosition();

        RTMapperNetHeader header;
        header.timestamp= GSLAM::TicToc::timestamp();
        header.cam_pitch = gimbal.pitch;
        header.cam_yaw = gimbal.yaw;
        header.cam_roll = gimbal.roll;
        header.lat= gps.latitude*180/3.1415926;
        header.lng= gps.longitude*180/3.1415926;
        header.alt= gps.altitude;
        header.H  = gps.height;
        header.payload_size=bufLen;
        header.video_codec=options.get("video_codec",4);

        if(camera_name.size()>20) camera_name.resize(20);

        strcpy(header.uavname,camera_name.c_str());

        // send header and data to clients
        DLOG(INFO)<<"Sending:"<<sv::Svar({{"cam_pitch",(double)header.cam_pitch},
                                         {"cam_yaw",(double)header.cam_yaw},
                                         {"cam_roll",(double)header.cam_roll},
                                         {"lat",(double)header.lat},
                                         {"lng",(double)header.lng},
                                         {"alt",(double)header.alt},
                                         {"camera_name",std::string(header.uavname)},
                                         {"payload_size",(int)header.payload_size}}).dump_json();

        pub_rtmv.publish(SvarBuffer(&header,sizeof(header)));
        pub_rtmv.publish(SvarBuffer(buf,bufLen));
    }

    void newPhotoNotify(){
        shouldStop=false;

        PayloadIndexType p_index= options.get("playload_index",PAYLOAD_INDEX_0);
        std::string      file_filter = options.get<std::string>("file_filter","Wide");
        std::string      cache_folder= options.get<std::string>("cache_folder","./");
        auto self=vehicle->cameraManager;

        LOG(INFO)<<"newPhotoNotify started for "<<file_filter;
        while(!shouldStop){
            if(0!=self->setModeSync(p_index,CameraModule::MEDIA_DOWNLOAD,2)) 
	{
	  LOG(INFO)<<"Failed to setModeSync";
	  continue;
	}
            if(0!=self->obtainDownloadRightSync(p_index,true,2)) {
	  LOG(INFO)<<"Failed to obtainDownloadRightSync";
	  continue;
	}

            std::promise<FilePackage> ret_filelist;
            self->startReqFileList(p_index,[](E_OsdkStat ret_code, const FilePackage file_list, void* userData){
                std::promise<FilePackage>& ret=*(std::promise<FilePackage>*)userData;
                ret.set_value(file_list);
            },&ret_filelist);

            auto f=ret_filelist.get_future();
            f.wait();

            FilePackage filelist= f.get();

            if(filelist.media.empty()) {
	  LOG(INFO)<<"Failed to obtain file list.";
	  continue;
	}

            for(MediaFile& f:filelist.media){
                if(downloaded.count(f.fileIndex)) continue;
                if(f.fileName.find(file_filter) == std::string::npos) continue;

                while(!downloadAndNotify(p_index,f,cache_folder)){
                    LOG(WARNING) << "Failed to download "<<f.fileName<<", retrying...";
                }

                downloaded.insert(f.fileIndex);

                LOG(INFO)<<"Downloaded "<<std::string(cache_folder+f.fileName);
                messenger.publish("downloaded_images",std::string(cache_folder+f.fileName));
            }
        }
    }

    bool downloadAndNotify(PayloadIndexType p_index, MediaFile& f, std::string local_folder){
        std::promise<int> ret;

        vehicle->cameraManager->startReqFileData(p_index, f.fileIndex,local_folder+f.fileName,
                                                 [](E_OsdkStat ret_code, void* userData){
            std::promise<int>& ret=*(std::promise<int>*)userData;
            ret.set_value((int)ret_code);
        },&ret);

        auto fu=ret.get_future();
        fu.wait();

        return fu.get() == 0;
    }

    bool     shouldStop;
    Vehicle* vehicle;
    sv::Svar options;
    sv::Svar data;
    msg::ThreadPool workers;
    std::set<int> downloaded;
    std::string camera_name;

    Publisher pub_rtmv;
};

class RTMVReader{
public:
    RTMVReader(std::string path):path(path),ifs(path){}

    sv::Svar grab(){
        RTMapperNetHeader header;
        ifs.read((char*)&header,sizeof(header));

        if(!ifs.good()) return sv::Svar();

        if(!header.isValid())
        {
            LOG(WARNING)<<"not valid";
            return sv::Svar();
        }

        sv::Svar ret;
        sv::SvarBuffer payload(header.payload_size);
//        LOG(INFO)<<"payload size:"<<header.payload_size;
        ifs.read((char*)payload.ptr(),payload.size());

        if(!ifs.good()) return sv::Svar();


        ret["payload"]=payload;
        ret["alt"]=(double)header.alt;

        return ret;
    }

    std::string path;
    std::ifstream ifs;
};

REGISTER_SVAR_MODULE(vehicle_node){
    Class<VehicleNode>("VehicleNode")
            .unique_construct<Vehicle*,Svar>()
            .def("newPhotoNotify",&VehicleNode::newPhotoNotify);

    Class<RTMVReader>("RTMVReader")
            .unique_construct<std::string>()
            .def("grab",&RTMVReader::grab)
            ;
}
